//SCAN_NODE
//Stores a TF message about the position,
//a laser scan at said position,
//and a cached grid rendition of said scan

#include "scan_node.hpp"
#include <math.h>
#include "ros/ros.h"
#include "ros/console.h"
#include "tf/tf.h"
#include "mapstore.hpp"

#define M_2PI 2*M_PI

double Scan_node::resolution;
double Scan_node::maxRange;

void Scan_node::addScan(sensor_msgs::LaserScan newScan) {
    addScan(newScan, true);
}

void Scan_node::addScan(sensor_msgs::LaserScan newScan, bool shouldUpdateRender) {
    if(number_of_scans == 0) {
        this->scan = newScan;
        number_of_scans = 1;
    }
    else {
        number_of_scans++;
        for(unsigned int i=0; i<scan.ranges.size(); i++) {
            if(newScan.intensities[i] != 0) {
                scan.ranges[i] *= (double)(number_of_scans-1);
                scan.ranges[i] += newScan.ranges[i];
                scan.ranges[i] /= (double)number_of_scans;
            }
        }
    }
    if(scan.ranges.size() > 0 && shouldUpdateRender) updateRender();

    if(storePtr != NULL && shouldUpdateRender) {
        storePtr ->dataChangedCallback(this);
    }
}

void Scan_node::setTransform(geometry_msgs::TransformStamped newTransform) {
    geometry_msgs::TransformStamped oldTransform = this->transform;
	this->transform = newTransform;
	if(scan.ranges.size() > 0) updateRender();

    if(storePtr != NULL) {
        storePtr->tfChangedCallback(this, oldTransform, newTransform);
    }
}

geometry_msgs::TransformStamped Scan_node::getTransform() {
	return transform;
}

void Scan_node::updateRender() {
	ROS_INFO("Updating render...");
	//Initialize render vector and calculate some basic data
	render.clear();
	ROS_INFO("Render cleared...");
	ROS_INFO("Scan max range: %f, cutoff: %f", scan.range_max, maxRange);
	ROS_INFO("Resolution: %f", resolution);
	offset = (int)ceilf(maxRange / resolution);
	int renderSize = 2*offset+1;
	
	for(int x=0; x<renderSize; x++) {
		std::vector<int> renderCol;
		for(int y=0; y<renderSize; y++) {
			renderCol.push_back(-1);
		}
		render.push_back(renderCol);
	}
	
	ROS_INFO("Render set up (size: %d)", renderSize);
	
	double yaw = tf::getYaw(transform.transform.rotation);
	
	ROS_INFO("Got yaw: %f (%f°)", yaw, (yaw/M_PI)*180.0);
	//First, draw the lines from the first and the last scans to the midpoint
	
	int rangeCount = scan.ranges.size();
	ROS_INFO("Ranges: %d", rangeCount);
	
	int frIndex = 0;
	double firstRange = scan.ranges[0];
	while((firstRange > scan.range_max || firstRange < scan.range_min) && frIndex < rangeCount) {
		frIndex++;
		firstRange = scan.ranges[frIndex];
	}
	int lrIndex = 0;
	double lastRange = scan.ranges[rangeCount-1];
	while((lastRange > scan.range_max || lastRange < scan.range_min) && lrIndex < rangeCount-1 ){
		lrIndex++;
		lastRange = scan.ranges[rangeCount - 1 - lrIndex];
	}
	double firstAngle = scan.angle_min + frIndex * scan.angle_increment + yaw;
	double lastAngle = scan.angle_max - (lrIndex+1) * scan.angle_increment + yaw;
	while(firstAngle >= M_2PI) {
		firstAngle -= M_2PI;
	}
	while(firstAngle < 0) {
		firstAngle += M_2PI;
	}
	while(lastAngle >= M_2PI) {
		lastAngle -= M_2PI;
	}
	while(lastAngle < 0) {
		lastAngle += M_2PI;
	}
	
	if(firstRange > maxRange) firstRange = maxRange;
	if(lastRange > maxRange) lastRange = maxRange;
	
	ROS_INFO("First angle: %f, last angle: %f (%f°, %f°)", firstAngle, lastAngle, firstAngle*180.0/M_PI, lastAngle*180.0/M_PI);
	
	int firstX = (int)roundf((fastCos(firstAngle) * firstRange) / resolution)+offset;
	int firstY = (int)roundf((fastSin(firstAngle) * firstRange) / resolution)+offset;
	int lastX = (int)roundf((fastCos(lastAngle) * lastRange) / resolution)+offset;
	int lastY = (int)roundf((fastSin(lastAngle) * lastRange) / resolution)+offset;
	
    /*double firstVecX = cos(firstAngle + M_PI);
	double firstVecY = sin(firstAngle + M_PI);
	double lastVecX = cos(lastAngle);
    double lastVecY = sin(lastAngle);*/
	
	if(firstX < 0) firstX = 0;
	if(firstY < 0) firstY = 0;
	if(lastX < 0) lastX = 0;
	if(lastY < 0) lastY = 0;
	if(firstX > renderSize-1) firstX = renderSize-1;
	if(firstY > renderSize-1) firstY = renderSize-1;
	if(lastX > renderSize-1) lastX = renderSize-1;
	if(lastY > renderSize-1) lastY = renderSize-1;
	
    //double c_x = firstX;
    //double c_y = firstY;
	
    ROS_DEBUG("First: (%d, %d), last: (%d, %d), center: (%d, %d)", firstX, firstY, lastX, lastY, offset, offset);
    ROS_DEBUG("First range: %f, last range: %f", firstRange, lastRange);
	
    /*
	do {
		
		render[roundf(c_x)][roundf(c_y)] = 0;
		
		c_x+=firstVecX;
		render[roundf(c_x)][roundf(c_y)] = 0;
		c_y+=firstVecY;
	} while(roundf(c_x)!=offset && roundf(c_y)!=offset);
	
	while(roundf(c_x)!=lastX && roundf(c_y)!=lastY) {
		
		render[roundf(c_x)][roundf(c_y)] = 0;
		
		c_x+=lastVecX;
		render[roundf(c_x)][roundf(c_y)] = 0;
		c_y+=lastVecY;
	}
    */
	
	//Draw the endpoints. 100 if the range is within the maxRange cutoff, 0 if it's outside that.
	
	double cAngle = firstAngle;

    int latestX = firstX;
    int latestY = firstY;
    int latestColor = (firstRange >= maxRange ? 0 : 100);
		
	for(int i=frIndex; i<rangeCount-lrIndex; i++) {
		
		double cRange = scan.ranges[i];
		
		if(cRange >= scan.range_min && cRange <= scan.range_max) {
			if(cRange > maxRange) cRange = maxRange;
		
			int x = roundf(fastCos(cAngle) * cRange / resolution)+offset;
			int y = roundf(fastSin(cAngle) * cRange / resolution)+offset;
		
			if(x<0) x=0;
			if(y<0) y=0;
			if(x>renderSize-1) x=renderSize-1;
			if(y>renderSize-1) y=renderSize-1;
			
            bresenham(x, y, offset, offset, render, 0, -1, false);
		
			if(cRange >= Scan_node::maxRange) {
				render[x][y] = 0;
                bresenham(x, y, latestX, latestY, render, latestColor/2, 0, false);
                latestColor = 0;
			}
			else {
				render[x][y] = 100;
                bresenham(x, y, latestX, latestY, render, latestColor/2+50, 0, false);
                latestColor = 100;
			}

            latestX = x;
            latestY = y;
		}
		cAngle += scan.angle_increment;
		if(cAngle > 2*M_PI) {
			cAngle -= 2*M_PI;
		}
	}
	//Completed. By this point the render contains the latest update for this node.
}

void Scan_node::bresenham(int from_x, int from_y, int to_x, int to_y, std::vector<std::vector<int> >& on, int ink, int background, bool overwrite) {
		// Bresenham's line algorithm
		
		double x1 = from_x;
		double y1 = from_y;
		double x2 = to_x;
		double y2 = to_y;
		
		const bool steep = (fabs(y2 - y1) > fabs(x2 - x1));
		if(steep)
		{
			std::swap(x1, y1);
			std::swap(x2, y2);
		}
		
		if(x1 > x2)
		{
			std::swap(x1, x2);
			std::swap(y1, y2);
		}
		
		const float dx = x2 - x1;
		const float dy = fabs(y2 - y1);
		
		float error = dx / 2.0f;
		const int ystep = (y1 < y2) ? 1 : -1;
		int y = (int)y1;
		
		const int maxX = (int)x2;
		
		for(int x=(int)x1; x<maxX; x++)
		{
			if(steep)
			{
                if(overwrite || on[y][x] < 0) on[y][x] = ink;
			}
			else
			{
                if(overwrite || on[x][y] < 0) on[x][y] = ink;
			}
			
			error -= dy;
			if(error < 0)
			{
				y += ystep;
				error += dx;
			}
		}
	}
	
double Scan_node::fastSin(double x ){
    x = fmod(x + M_PI, M_PI * 2) - M_PI; // restrict x so that -M_PI < x < M_PI
    const float B = 4.0f/M_PI;
    const float C = -4.0f/(M_PI*M_PI);

    float y = B * x + C * x * std::abs(x);

    const float P = 0.225f;

    return P * (y * std::abs(y) - y) + y; 
}

double Scan_node::fastCos(double x) {
	return fastSin(x+M_PI_2);
}

Scan_node::Scan_node() {
    number_of_scans = 0;
    storePtr = NULL;
}

void Scan_node::setResolution(double newResolution) {
	resolution = newResolution;
}

void Scan_node::setMaxRange(double newMaxRange) {
	maxRange = newMaxRange;
}

Scan_node::Scan_node(sensor_msgs::LaserScan scan, geometry_msgs::TransformStamped transform) {
	this->resolution = resolution;
	this->scan = scan;
	this->transform = transform;
    number_of_scans = 1;
    updateRender();
}

int Scan_node::getNumberOfScans() {
    return this->number_of_scans;
}
